import numpy as np
import matplotlib.pyplot as plt

def generate_ramp_signal(points, num_steps):
    signal = []
    for i in range(len(points) - 1):
        start = np.array(points[i])
        end = np.array(points[i+1])
        for t in np.linspace(0, 1, num_steps):
            signal.append((1 - t) * start + t * end)
    return np.array(signal)

if __name__ == '__main__':
    points = [
        [0, 0, 0],
        [0, 0, -5],
        [5, 0, -5],
        [5, 5, -5],
        [0, 5, -5],
        [0, 0, -5],
        [0, 0, 0]
    ]
    # 每段插值的步数
    num_steps = 100

    # 生成斜坡信号
    ramp_signal = generate_ramp_signal(points, num_steps)

    # 提取各个坐标轴的信号
    x_signal = ramp_signal[:, 0]
    y_signal = ramp_signal[:, 1]
    z_signal = ramp_signal[:, 2]

    fig = plt.figure()
    ax = fig.add_subplot(121, projection='3d')
    ax.plot(x_signal, y_signal, z_signal, label='Ramp Signal')
    ax.scatter(*zip(*points), color='red')  # 标出拐点
    ax.legend()
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax = fig.add_subplot(122)
    ax.plot([i for i in range(len(x_signal))],x_signal)
    plt.show()
